#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <unistd.h>
#include <string.h>
#include <sys/time.h>
#include <time.h>
#define __USE_GNU
#include <pthread.h>
#include <math.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>  //PISOX中定义的标准接口

#include "ethercat.h"

#define NSEC_PER_SEC 1000000000
#define EC_TIMEOUTMON 500

#define EEP_MAN_SYNAPTICON (0x000022d2)
#define EEP_ID_SYNAPTICON (0x00000201)

struct sched_param schedp;
char IOmap[4096];
pthread_t thread1, thread2;
uint64_t diff, maxt, avg, cycle;
struct timeval t1, t2;
int dorun = 0;
int64 toff = 0;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;

enum {
  STATE_RESET,
  STATE_INIT,
  STATE_PREREADY,
  STATE_READY,
  STATE_PREENABLE,
  STATE_ENABLE,
  STATE_DISABLE
};

int states[50];

/*****************************************************************************/
/**
   statusword bit definition.
*/
typedef struct _stswrd_bits_t {
  uint8_t rtso : 1;  // BIT0: ready to switch on
  uint8_t so : 1;    // BIT1: switched on
  uint8_t oe : 1;    // BIT2: operation enabled
  uint8_t f : 1;     // BIT3: fault
  uint8_t ve : 1;    // BIT4:  voltage enabled
  uint8_t qs : 1;    // BIT5: quick stop
  uint8_t sod : 1;   // BIT6: switch on disabled
  uint8_t w : 1;     // BIT7: warning
  uint8_t ms1 : 1;   // BIT8: manufacturer-specific1
  uint8_t rm : 1;    // BIT9: remote
  uint8_t tr : 1;    // BIT10: target reached
  uint8_t ila : 1;   // BIT11:  internal limit active
  uint8_t oms1 : 1;  // BIT12: oms1 pp-Set-point acknowledge; hm-Homing attained;
                     //		ip-ip  mode active;csp-Target position ignored;
                     //		csv-Target velocity ignored;cst-Target torque ignored;
  uint8_t oms2 : 1;  // BIT13: oms1 pp-Following error; hm-Homing error; csp-Following error;
  uint8_t ms2 : 1;   // BIT14: manufacturer-specific2
  uint8_t ms3 : 1;   // BIT15: manufacturer-specific3
} stswrd_bits_t;
/*****************************************************************************/
/**
   statusword definition.
*/
typedef union _stswrd_t {
  uint16_t all;
  stswrd_bits_t bit;
} stswrd_t;
/*****************************************************************************/

typedef struct PACKED {
  uint16_t Controlword;
  int32_t TargetPos;
  int32_t TargetVel;
  int16_t TargetTor;
  uint8_t ModeOp;
} Drive_Outputs;

typedef struct PACKED {
  uint16_t Statusword;
  int32_t ActualPos;
  int32_t ActualVel;
  int16_t ActualTor;
  uint8_t ModeOp;
} Drive_Inputs;

static int drive_write8(uint16 slave, uint16 index, uint8 subindex, uint8 value) {
  int wkc;

  wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

  return wkc;
}

static int drive_write16(uint16 slave, uint16 index, uint8 subindex, uint16 value) {
  int wkc;

  wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

  return wkc;
}

static int drive_write32(uint16 slave, uint16 index, uint8 subindex, int32 value) {
  int wkc;

  wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

  return wkc;
}

// 该函数用于设置PDO映射表
int drive_setup(uint16 slave) {
  int wkc = 0;

  printf("Drive setup\n");

  wkc += drive_write16(slave, 0x1C12, 0, 0);
  wkc += drive_write16(slave, 0x1C13, 0, 0);

  wkc += drive_write16(slave, 0x1A00, 0, 0);
  wkc += drive_write32(slave, 0x1A00, 1, 0x60410010);  // Statusword
  wkc += drive_write32(slave, 0x1A00, 2, 0x60640020);  // Position actual value
  wkc += drive_write32(slave, 0x1A00, 3, 0x606C0020);  // Velocity actual value
  wkc += drive_write32(slave, 0x1A00, 4, 0x60770010);  // Torque actual value
  wkc += drive_write32(slave, 0x1A00, 5, 0x60610008);  // Modes of operation display
  wkc += drive_write8(slave, 0x1A00, 0, 5);

  wkc += drive_write8(slave, 0x1600, 0, 0);
  wkc += drive_write32(slave, 0x1600, 1, 0x60400010);  // Controlword
  wkc += drive_write32(slave, 0x1600, 2, 0x607A0020);  // Target position
  wkc += drive_write32(slave, 0x1600, 3, 0x60FF0020);  // Target velocity
  wkc += drive_write32(slave, 0x1600, 4, 0x60710010);  // Target torque
  wkc += drive_write32(slave, 0x1600, 5, 0x60600008);  // Modes of operation display
  wkc += drive_write8(slave, 0x1600, 0, 5);

  wkc += drive_write16(slave, 0x1C12, 1, 0x1600);
  wkc += drive_write8(slave, 0x1C12, 0, 1);

  wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
  wkc += drive_write8(slave, 0x1C13, 0, 1);

  strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);

  if (wkc != 20) {
    printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
    return -1;
  } else
    printf("Drive %d setup succeed.\n", slave);

  return 0;
}

/* add ns to timespec */
void add_timespec(struct timespec* ts, int64 addtime) {
  int64 sec, nsec;

  nsec = addtime % NSEC_PER_SEC;
  sec = (addtime - nsec) / NSEC_PER_SEC;
  ts->tv_sec += sec;
  ts->tv_nsec += nsec;
  if (ts->tv_nsec > NSEC_PER_SEC) {
    nsec = ts->tv_nsec % NSEC_PER_SEC;
    ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
    ts->tv_nsec = nsec;
  }
}

/* PI calculation to get linux time synced to DC time */
void ec_sync(int64 reftime, int64 cycletime, int64* offsettime) {
  static int64 integral = 0;
  int64 delta;
  /* set linux sync point 50us later than DC sync, just as example */
  delta = (reftime - 50000) % cycletime;
  if (delta > (cycletime / 2)) {
    delta = delta - cycletime;
  }
  if (delta > 0) {
    integral++;
  }
  if (delta < 0) {
    integral--;
  }
  *offsettime = -(delta / 100) - (integral / 20);
}

static inline int64_t calcdiff_ns(struct timespec t1, struct timespec t2) {
  int64_t tdiff;
  tdiff = NSEC_PER_SEC * (int64_t)((int)t1.tv_sec - (int)t2.tv_sec);
  tdiff += ((int)t1.tv_nsec - (int)t2.tv_nsec);
  return tdiff;
}

static int latency_target_fd = -1;
static int32_t latency_target_value = 0;


void test_driver(char* ifname) {
  int cnt, ccnt, i, j;
  Drive_Inputs* iptr;
  Drive_Outputs* optr;

  cpu_set_t mask;
  pthread_t thread;
  int ht;
  int chk = 2000;
  int64 cycletime;
  struct timespec ts, tnow;
  union _stswrd_t status;
  int state;




  /* initialise SOEM, bind socket to ifname */
  if (ec_init(ifname)) {
    printf("ec_init on %s succeeded.\n", ifname);
    /* find and auto-config slaves */
    if (ec_config_init(FALSE) > 0) {
      ecx_context.manualstatechange = 1;
      printf("%d slaves found and configured.\n", ec_slavecount);
      /* wait for all slaves to reach SAFE_OP state */

      int slave_ix;
      for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++) {
        ec_slavet* slave = &ec_slave[slave_ix];
        slave->PO2SOconfig = drive_setup;
      }


      ec_config_map(&IOmap);  // 此处调用drive_setup函数，进行PDO映射表设置
      ec_configdc();          // 设置同步时钟，该函数必须在设置pdo映射之后；

      // setup dc for devices
      for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++) {
        ec_dcsync0(slave_ix, TRUE, 8000000U, 20000U);
      }

      printf("Slaves mapped, state to SAFE_OP.\n");
      ec_slave[0].state = EC_STATE_SAFE_OP;
      /* send one valid process data to make outputs in slaves happy*/
      
      /* request OP state for all slaves */
      ec_writestate(0);
      ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);

      /* read indevidual slave state and store in ec_slave[] */

      ec_readstate();
      for (cnt = 1; cnt <= ec_slavecount; cnt++) {
        printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
               cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
               ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
      }
      expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
      printf("Calculated workcounter %d\n", expectedWKC);

      printf("Request operational state for all slaves\n");
      /* activate cyclic process data */
      /* wait for all slaves to reach OP state */
      ec_slave[0].state = EC_STATE_OPERATIONAL;
      /* request OP state for all slaves */
      ec_writestate(0);

      clock_gettime(CLOCK_MONOTONIC, &ts);
      ht = (ts.tv_nsec / 1000000) + 20; /* round to nearest 20 ms */
      ts.tv_nsec = ht * 1000000;
      cycletime = 8000 * 1000; /* cycletime in ns */

      /* 对PDO进行初始化 */
      for (i = 0; i < ec_slavecount; i++) {
        optr = (Drive_Outputs*)ec_slave[i + 1].outputs;
        if (optr == NULL) {
          printf("optr is NULL.\n");
        }
        optr->Controlword = 0;
        optr->TargetPos = 0;
        optr->ModeOp = 0;
        optr->TargetTor = 0;
        optr->TargetVel = 0;
      }

      do {
        /* wait to cycle start */
        clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL);
        if (ec_slave[0].hasdc) {
          /* calulate toff to get linux time and DC synced */
          ec_sync(ec_DCtime, cycletime, &toff);
        }
        wkc = ec_receive_processdata(EC_TIMEOUTRET);
        ec_send_processdata();
        add_timespec(&ts, cycletime + toff);
      } while (chk-- && (wkc != expectedWKC));
      /* 此处与SOEM官方例程不一样，因为ec_statecheck函数消耗的时间较多，有可能超过循环周期 */

      if (wkc == expectedWKC) {
        printf("Operational state reached for all slaves.\n");
        inOP = TRUE;
        cnt = 0;
        while (cnt < 100) {
          /* 计算下一周期唤醒时间 */
          add_timespec(&ts, cycletime + toff);
          /* wait to cycle start */
          clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL);
          clock_gettime(CLOCK_MONOTONIC, &tnow);
          if (ec_slave[0].hasdc) {
            /* calulate toff to get linux time and DC synced */
            ec_sync(ec_DCtime, cycletime, &toff);
          }
          wkc = ec_receive_processdata(EC_TIMEOUTRET);
          diff = calcdiff_ns(tnow, ts);

          for (i = 0; i < ec_slavecount; i++) {
            optr = (Drive_Outputs*)ec_slave[i + 1].outputs;
            iptr = (Drive_Inputs*)ec_slave[i + 1].inputs;
            status.all = iptr->Statusword;
            state = states[i];

            switch (state) {
              case STATE_RESET:
                optr->Controlword = 6;
                optr->TargetPos = iptr->ActualPos;
                optr->ModeOp = 0;

                states[i] = STATE_INIT;
                break;
              case STATE_INIT /* 初始化驱动器,清除错误 */:
                optr->Controlword = 134;
                optr->TargetPos = iptr->ActualPos;

                if (status.bit.sod || status.bit.so || status.bit.rtso) {
                  optr->ModeOp = 8;
                  states[i] = STATE_PREREADY;
                }
                break;
              case STATE_PREREADY:
                optr->Controlword = 6;
                optr->ModeOp = 8;
                optr->TargetPos = iptr->ActualPos;

                if (status.bit.rtso && status.bit.qs) {
                  ccnt++;
                  if (ccnt > 200 * ec_slavecount)
                  {
                    /* code */
                    if (i+1 == ec_slavecount) {
                      ccnt = 0;
                    }

                    states[i] = STATE_READY;
                  }
                }
                break;
              case STATE_READY /* 系统转为准备使能状态 */:
                optr->Controlword = 7;
                optr->ModeOp = 8;
                optr->TargetPos = iptr->ActualPos;

                if (status.bit.rtso && status.bit.qs && status.bit.so) {
                  ccnt++;
                  if (ccnt > 220 * ec_slavecount)
                  {
                    /* code */
                    if(i+1 == ec_slavecount){
                      ccnt = 0;
                    }

                    states[i] = STATE_PREENABLE;
                  }
                }
                break;
              case STATE_PREENABLE /* 驱动器使能 */:
                optr->Controlword = 15;
                optr->ModeOp = 8;

                if (status.bit.rtso && status.bit.qs && status.bit.so && status.bit.oe) {
                  states[i] = STATE_ENABLE;
                }
                break;
              case STATE_ENABLE /* 驱动器运动 */:
                // optr->Controlword = 31;
                optr->ModeOp = 8;
                // optr->TargetPos += 2000;
                // optr->TargetVel = 2000;
                break;
              case STATE_DISABLE:
                /* 使电机失能并在cnt==100退出循环 */
                optr->ModeOp = 0;
                optr->TargetVel = 0;
                optr->Controlword = 6;

                cnt++;
                break;
              default:
                break;
            }
          }
          ec_send_processdata();
          cycle++;

          avg += diff;
          if (diff > maxt)
            maxt = diff;

          for (j = 0; j < 3; j++) {
            iptr = (Drive_Inputs*)ec_slave[j + 1].inputs;
            optr = (Drive_Outputs*)ec_slave[j + 1].outputs;
            //   printf("  %d: pos: %d", j + 1, iptr->ActualPos);
            printf(" %d:CW:%d,SW:%d, Md: %d", j + 1, optr->Controlword, iptr->Statusword, iptr->ModeOp);

          }
          printf(", MaxLatency: %lu, avg: %lu    \r", maxt, avg / cycle);
          fflush(stdout);
        }
        dorun = 0;
      } else /* ECAT进入OP失败 */
      {
        printf("Not all slaves reached operational state.\n");
        ec_readstate();
        for (i = 1; i <= ec_slavecount; i++) {
          if (ec_slave[i].state != EC_STATE_OPERATIONAL) {
            printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                   i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
          }
        }
      }
      /* 断开ECAT通讯 */
      printf("\nRequest safe operational state for all slaves\n");
      ec_slave[0].state = EC_STATE_SAFE_OP;
      /* request SAFE_OP state for all slaves */
      ec_writestate(0);
      ec_slave[0].state = EC_STATE_PRE_OP;
      ec_writestate(0);
      ec_slave[0].state = EC_STATE_INIT;
      ec_writestate(0);
      ec_readstate();
      if (ec_statecheck(0, EC_STATE_SAFE_OP, 1000) == EC_STATE_INIT) {
        printf("ECAT changed into state init\n");
      }
    } else {
      printf("No slaves found!\n");
    }
    printf("End driver test, close socket\n");
    /* stop SOEM, close socket */
    ec_close();
  } else {
    printf("No socket connection on %s\nExcecute as root\n", ifname);
  }
}

int main(int argc, char* argv[]) {
  printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n");
  if (argc > 1) {
    dorun = 0;





    /* start acyclic part */
    test_driver(argv[1]);
  } else {
    printf("Usage: motion_test ifname1 Mode_of_operation\nifname = eth0 for example\n");
  }

  printf("End program\n");

  return (0);
}
